#include "GPlane.h"
#include "GMath.h"

namespace GMath
{

	GPlane::GPlane()
	{
		Point = GVector3::Zero;
		Normal = GVector3::UnitZ;
	}

	// Normal may be showing the opposite way, user should manage it
	GPlane::GPlane(float A, float B, float C, float D)
	{
		GVector3 Px(-D / A, 0.0f, 0.0f);
		GVector3 Py(0.0f, -D / B, 0.0f);
		GVector3 Pz(0.0f, 0.0f, -D / C);

		GVector3 V0 = Py - Px;
		GVector3 V1 = Pz - Px;

		GVector3::Cross(Normal, V0, V1);
		Normal.Normalize();

		Point = (Px + Py + Pz) / 3.0f;
	}

	GPlane::GPlane(const GVector3& Point, const GVector3& Normal)
	{
		//! warning if normal is not normalized
		this->Point = Point;
		this->Normal = Normal;
	}

	GPlane::GPlane(const GVector3& Point0, const GVector3& Point1, const GVector3& Point2)
	{
		Point = Point0;

		GVector3 Vec1 = Point1 - Point0;
		GVector3 Vec2 = Point2 - Point0;

		Vec1.Normalize();
		Vec2.Normalize();

		GVector3::Cross(Normal, Vec2, Vec1);
		Normal.Normalize();
	}

	void GPlane::Create(float A, float B, float C, float D)
	{
		GVector3 Px(-D / A, 0.0f, 0.0f);
		GVector3 Py(0.0f, -D / B, 0.0f);
		GVector3 Pz(0.0f, 0.0f, -D / C);

		GVector3 V0 = Py - Px;
		GVector3 V1 = Pz - Px;

		GVector3::Cross(Normal, V0, V1);
		Normal.Normalize();

		Point = (Px + Py + Pz) / 3.0f;
	}

	void GPlane::Create(const GVector3& Point, const GVector3& Normal)
	{
		this->Point = Point;
		this->Normal = Normal;
	}

	void GPlane::Create(const GVector3& Point0, const GVector3& Point1, const GVector3& Point2)
	{
		Point = Point0;

		GVector3 Vec1 = Point1 - Point0;
		GVector3 Vec2 = Point2 - Point0;

		Vec1.Normalize();
		Vec2.Normalize();

		GVector3::Cross(Normal, Vec2, Vec1);
		Normal.Normalize();
	}

	void GPlane::GetCoefficients(OUTPUT float& A, OUTPUT float& B, OUTPUT float& C, OUTPUT float& D) const
	{
		A = Normal.x;
		B = Normal.y;
		C = Normal.z;
		D = GVector3::Dot(Normal, Point);
	}

	GHalfSpaceResult GPlane::HalfSpace(const GVector3& TestPoint)
	{
		GVector3 Vec = TestPoint - Point;
		float SignedDist = Normal.Dot(Vec);
	
		if (SignedDist < -G_ZERO_TRESHOLD )
		{
			return G_HSR_NEGATIVE;
		}
		else if (SignedDist > G_ZERO_TRESHOLD)
		{
			return G_HSR_POSITIVE;
		}
	
		return G_HSR_ON_PLANE;
	}

	float GPlane::Distance(const GVector3& Point)
	{
		GVector3 Vec = Point - this->Point;
		return AbsF(Normal.Dot(Vec));
	}

}